//
// Created by ZhaoXiaoFei on 2022/7/28.
//

#ifndef VINS_ESTIMATOR_IMU_FACTOR_HPP
#define VINS_ESTIMATOR_IMU_FACTOR_HPP

#include <iostream>
#include <eigen3/Eigen/Dense>

#include "../utility/utility.h"
#include "../parameters.h"
#include "preIntegration.hpp"

#include <ceres/ceres.h>

class IMUFaxtor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>{
public:
    IMUFaxtor() = delete;
    IMUFaxtor(IMUpreintegration* imUpreintegration_) : imUpreintegration(imUpreintegration_){
    }
    virtual bool Evaluate(double const* const* parameters,
                          double* residuals,
                          double** jacobians) const {
        Eigen::Vector3d Pi = Eigen::Vector3d(parameters[0][0], parameters[0][1], parameters[0][2]);
        Eigen::Quaterniond Qi = Eigen::Quaterniond(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

        Eigen::Vector3d Vi = Eigen::Vector3d(parameters[1][0], parameters[1][1], parameters[1][2]);
        Eigen::Vector3d Bai = Eigen::Vector3d(parameters[1][3], parameters[1][4], parameters[1][5]);
        Eigen::Vector3d Bgi = Eigen::Vector3d(parameters[1][6], parameters[1][7], parameters[1][8]);

        Eigen::Vector3d Pj = Eigen::Vector3d(parameters[2][0], parameters[2][1], parameters[2][2]);
        Eigen::Quaterniond Qj = Eigen::Quaterniond(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

        Eigen::Vector3d Vj = Eigen::Vector3d(parameters[3][0], parameters[3][1], parameters[3][2]);
        Eigen::Vector3d Baj = Eigen::Vector3d(parameters[3][3], parameters[3][4], parameters[3][5]);
        Eigen::Vector3d Bgj = Eigen::Vector3d(parameters[3][6], parameters[3][7], parameters[3][8]);

        Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
        residual = imUpreintegration->evaluate(Pi, Qi, Vi,  Bai,  Bgi,
                                               Pj,  Qj,  Vj, Baj, Bgj);
        Eigen::Matrix<double,15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(imUpreintegration->covariance.inverse()).matrixL().transpose();
//        LOG(INFO) << "sqrt_info: "  << std::endl << sqrt_info << std::endl;
//        LOG(INFO) << "residual: "  << std::endl << residual.transpose() << std::endl;
        residual = sqrt_info * residual;


        if(jacobians){
            double dt = imUpreintegration->sum_dt;

            Eigen::Matrix3d dp_dba = imUpreintegration->jacobian.block<3,3>(0,9);
            Eigen::Matrix3d dp_dbg = imUpreintegration->jacobian.block<3,3>(0,12);

            Eigen::Matrix3d dq_dbg = imUpreintegration->jacobian.block<3,3>(3,12);

            Eigen::Matrix3d dv_dba = imUpreintegration->jacobian.block<3,3>(6,9);
            Eigen::Matrix3d dv_dbg = imUpreintegration->jacobian.block<3,3>(6,12);

//            Eigen::Vector3d dba = Bai - imUpreintegration->linearized_ba;
//            Eigen::Vector3d dbg = Bgi - imUpreintegration->linearized_ba;
            if(jacobians[0]){
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
                jacobian_pose_i.setZero();
                jacobian_pose_i.block<3, 3>(0, 0) = -Qi.inverse().toRotationMatrix();
                jacobian_pose_i.block<3, 3>(0, 3) = Utility::skewSymmetric(Qi.inverse() * (Pj - Pi - Vi * dt + 0.5 * G * dt * dt));

                //Eigen::Quaterniond correctedQ = imUpreintegration->deltaQ * Eigen::Quaterniond(1, (0.5 * dq_dbg * dbg)(0), (0.5 * dq_dbg * dbg)(1), (0.5 * dq_dbg * dbg)(2));
                Eigen::Quaterniond correctedQ = imUpreintegration->deltaQ * Utility::deltaQ(dq_dbg * (Bgi - imUpreintegration->linearized_bg));
                jacobian_pose_i.block<3, 3>(3, 3) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(correctedQ)).bottomRightCorner<3, 3>();

                jacobian_pose_i.block<3, 3>(6, 3) = Utility::skewSymmetric(Qi.inverse() * (Vj - Vi + G * dt));
//                LOG(INFO) << "jacobian_pose_i: "  << std::endl << jacobian_pose_i << std::endl;
                jacobian_pose_i = sqrt_info * jacobian_pose_i;
            }
            if(jacobians[1]){
                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbais_i(jacobians[1]);
                jacobian_speedbais_i.setZero();

                jacobian_speedbais_i.block<3, 3>(0, 0) = -Qi.inverse().toRotationMatrix() * dt;
                jacobian_speedbais_i.block<3, 3>(0, 3) = -dp_dba;
                jacobian_speedbais_i.block<3, 3>(0, 6) = -dp_dbg;

//                Eigen::Quaterniond correctedQ = imUpreintegration->deltaQ * Eigen::Quaterniond(1, (0.5 * dq_dbg * dbg)(0), (0.5 * dq_dbg * dbg)(1), (0.5 * dq_dbg * dbg)(2));
//                jacobian_speedbais_i.block<3, 3>(3, 6) = Utility::Qleft(Qj.inverse() * Qi * correctedQ).bottomRightCorner<3, 3>() * dq_dbg;
                Eigen::Quaterniond correctedQ = imUpreintegration->deltaQ * Utility::deltaQ(dq_dbg * (Bgi - imUpreintegration->linearized_bg));
                jacobian_speedbais_i.block<3, 3>(3, 6) = -Utility::Qleft(Qj.inverse() * Qi * correctedQ).bottomRightCorner<3, 3>() * dq_dbg;

                jacobian_speedbais_i.block<3, 3>(6, 0) = -Qi.inverse().toRotationMatrix();
                jacobian_speedbais_i.block<3, 3>(6, 3) = -dv_dba;
                jacobian_speedbais_i.block<3, 3>(6, 6) = -dv_dbg;

                jacobian_speedbais_i.block<3, 3>(9, 3) = -Eigen::Matrix3d::Identity();
                jacobian_speedbais_i.block<3, 3>(12, 6) = -Eigen::Matrix3d::Identity();
//                LOG(INFO) << "jacobian_speedbais_i: "  << std::endl << jacobian_speedbais_i << std::endl;
                jacobian_speedbais_i = sqrt_info * jacobian_speedbais_i;

            }
            if(jacobians[2]){
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);
                jacobian_pose_j.setZero();

                jacobian_pose_j.block<3, 3>(0, 0) = Qi.inverse().toRotationMatrix();

//                Eigen::Quaterniond correctedQ = imUpreintegration->deltaQ * Eigen::Quaterniond(1, (0.5 * dq_dbg * dbg)(0), (0.5 * dq_dbg * dbg)(1), (0.5 * dq_dbg * dbg)(2));
                Eigen::Quaterniond correctedQ = imUpreintegration->deltaQ * Utility::deltaQ(dq_dbg * (Bgi - imUpreintegration->linearized_bg));
                jacobian_pose_j.block<3, 3>(3, 3) = Utility::Qleft(correctedQ.inverse() * Qi.inverse() * Qj).bottomRightCorner<3,3>();
//                LOG(INFO) << "jacobian_pose_j: "  << std::endl << jacobian_pose_j << std::endl;
                jacobian_pose_j = sqrt_info * jacobian_pose_j;

            }
            if(jacobians[3]){

                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbais_j(jacobians[3]);
                jacobian_speedbais_j.setZero();

                jacobian_speedbais_j.block<3, 3>(6, 0) = Qi.inverse().toRotationMatrix();
                jacobian_speedbais_j.block<3, 3>(9, 3) = Eigen::Matrix3d::Identity();
                jacobian_speedbais_j.block<3, 3>(12, 6) = Eigen::Matrix3d::Identity();

//                LOG(INFO) << "jacobian_speedbais_j: "  << std::endl << jacobian_speedbais_j << std::endl;

                jacobian_speedbais_j = sqrt_info * jacobian_speedbais_j;
            }
        }
        return true;
    }
    IMUpreintegration* imUpreintegration;
};

#endif //VINS_ESTIMATOR_IMU_FACTOR_HPP



